/* 
The NIST RCS (Real-time Control Systems) 
 library is public domain software, however it is preferred
 that the following disclaimers be attached.

Software Copywrite/Warranty Disclaimer

   This software was developed at the National Institute of Standards and
Technology by employees of the Federal Government in the course of their
official duties. Pursuant to title 17 Section 105 of the United States
Code this software is not subject to copyright protection and is in the
public domain. NIST Real-Time Control System software is an experimental
system. NIST assumes no responsibility whatsoever for its use by other
parties, and makes no guarantees, expressed or implied, about its
quality, reliability, or any other characteristic. We would appreciate
acknowledgement if the software is used. This software can be
redistributed and/or modified freely provided that any derivative works
bear some notice that they are derived from it, and any modified
versions bear some notice that they have been modified.



 */

 /*
*       New Java File starts here.
*       This file should be named PM_ROTATION_VECTOR.java
 */
// Set Package Name
package rcs.posemath;

// Import all NML, CMS, and RCS classes and interfaces
import rcs.nml.NMLFormatConverter;

/*
*       Class definition for PM_ROTATION_VECTOR
*       Automatically generated by RCS Java Diagnostics Tool.
*       on Wed Jan 07 10:53:42 EST 1998
 */
public class PM_ROTATION_VECTOR extends PmRotationVector implements Cloneable {

    public void update(NMLFormatConverter nml_fc) {
        super.update(nml_fc);
    }

    public PM_ROTATION_VECTOR() {
        super();
    }

    public PM_ROTATION_VECTOR(double starts, double startx, double starty, double startz) throws PmException {
        super(starts, startx, starty, startz);
    }

    public PM_ROTATION_VECTOR(PmRotationVector rv) throws PmException {
        super(rv.s, rv.x, rv.y, rv.z);
    }

    public PM_ROTATION_VECTOR(PmRotationMatrix rm) throws PmException {
        this(Posemath.toRot(rm));
    }

    public PM_ROTATION_VECTOR(PmRpy rpy) throws PmException {
        this(Posemath.toRot(rpy));
    }
    
    public PM_ROTATION_VECTOR(PmQuaternion quat) throws PmException {
        this(Posemath.toRot(quat));
    }
    
    public PM_ROTATION_VECTOR multiply(PmRotationVector other) throws PmException {
        PM_ROTATION_VECTOR ret = new PM_ROTATION_VECTOR();
        PmQuaternion qthis = Posemath.toQuat(this);
        PmQuaternion qother = Posemath.toQuat(other);
        PM_QUATERNION qret = new PM_QUATERNION();
        Posemath.pmQuatQuatMult(qthis, qother, qret);
        Posemath.pmQuatRotConvert(qret,ret);
        return ret;
    }
    
    public PM_ROTATION_VECTOR multiply(PmRotationMatrix other) throws PmException {
        PM_ROTATION_VECTOR ret = new PM_ROTATION_VECTOR();
        PmQuaternion qthis = Posemath.toQuat(this);
        PmQuaternion qother = Posemath.toQuat(other);
        PM_QUATERNION qret = new PM_QUATERNION();
        Posemath.pmQuatQuatMult(qthis, qother, qret);
        Posemath.pmQuatRotConvert(qret,ret);
        return ret;
    }
    
    public PM_ROTATION_VECTOR multiply(PmRpy other) throws PmException {
        PM_ROTATION_VECTOR ret = new PM_ROTATION_VECTOR();
        PmQuaternion qthis = Posemath.toQuat(this);
        PmQuaternion qother = Posemath.toQuat(other);
        PM_QUATERNION qret = new PM_QUATERNION();
        Posemath.pmQuatQuatMult(qthis, qother, qret);
        Posemath.pmQuatRotConvert(qret,ret);
        return ret;
    }
    
    public PM_ROTATION_VECTOR multiply(PmQuaternion qother) throws PmException {
        PM_ROTATION_VECTOR ret = new PM_ROTATION_VECTOR();
        PmQuaternion qthis = Posemath.toQuat(this);
        PM_QUATERNION qret = new PM_QUATERNION();
        Posemath.pmQuatQuatMult(qthis, qother, qret);
        Posemath.pmQuatRotConvert(qret,ret);
        return ret;
    }
    
    @Override
    public PM_ROTATION_VECTOR clone() {
        PM_ROTATION_VECTOR v = new PM_ROTATION_VECTOR();
        v.s = s;
        v.x = x;
        v.y = y;
        v.z = z;
        return v;
    }

    public double getRxDeg() {
        return Math.toDegrees(s * x);
    }

    public double getRyDeg() {
        return Math.toDegrees(s * y);
    }

    public double getRzDeg() {
        return Math.toDegrees(s * z);
    }
    
    

//    @Override
//    public String toString() {
//        return "PM_ROTATION_VECTOR{ Rx=" + getRxDeg() + "(deg) , Ry=" + getRyDeg() + " (deg)  , Rz=" + getRzDeg() + " (deg) }";
//    }

    @Override
    public String toString() {
        return "PM_ROTATION_VECTOR{s="+s+",x="+x+",y="+y+",z="+z + "Rx=" + getRxDeg() + "(deg) , Ry=" + getRyDeg() + " (deg)  , Rz=" + getRzDeg() + " (deg) }";
    }

}
